#ifndef __MOVETASK_H
#define __MOVETASK_H

#include "init.h"
#if INFANTRY==4
/* math relevant */
/* radian coefficient */
#define RADIAN_COEF        57.3f
#define chassis_rotate    0.65
chassis_t chassis;


/************************ chassis parameter ****************************/
/* the radius of wheel(mm) */
#define RADIUS                 76
/* the perimeter of wheel(mm) */
#define PERIMETER              478

/* wheel track distance(mm) */
#define WHEELTRACK             410
/* wheelbase distance(mm) */
#define WHEELBASE              390

#else
/* math relevant */
/* radian coefficient */
#define RADIAN_COEF        57.3f
#define chassis_rotate    0.65


/************************ chassis parameter ****************************/
/* the radius of wheel(mm) */
#define RADIUS                 76
/* the perimeter of wheel(mm) */
#define PERIMETER              478

/* wheel track distance(mm) */
#define WHEELTRACK             410
/* wheelbase distance(mm) */
#define WHEELBASE              340

#endif
/* gimbal is relative to chassis center x axis offset(mm) */
#define GIMBAL_X_OFFSET        0
/* gimbal is relative to chassis center y axis offset(mm) */
#define GIMBAL_Y_OFFSET        0


#define ANGLE_TO_RAD   0.01745329251994329576923690768489f
  /* chassis motor use 3508 */
  /* the deceleration ratio of chassis motor */
  #define CHASSIS_DECELE_RATIO (1.0f/19.0f)
  /* single 3508 motor maximum speed, unit is rpm */
  #define MAX_WHEEL_RPM        8500  //8347rpm = 3500mm/s
  /* chassis maximum translation speed, unit is mm/s */
  #define MAX_CHASSIS_VX_SPEED 4200  //8000rpm
  #define MAX_CHASSIS_VY_SPEED 4200
  /* chassis maximum rotation speed, unit is degree/s */
  #define MAX_CHASSIS_VR_SPEED 2000   //5000rpm
	/*---------------power-limit-----------------------*/
	#define WARNING_ENERGY 40.0f




void Move_task(void);
void mecanum_calc(float vx, float vy, float vw, int16_t speed[]);

void Chassis_Init(void);
void Chsiss_limit(void);

#endif
